/**
 * Implement the 3D predicates for primitive intersections. 
 * @author Songgang Xu (sxu at tamu dot edu)
 * @file predicate_3d.h
 */


#ifndef __PREDICATE_3D_H
#define __PREDICATE_3D_H

#include "predicate_2d.h"

namespace math 
{
extern const double tiny_value ;
/**
 * Decide the sign of det of 4 x 4. the vector are view as column vector. 
 * This is not robust if the computation is not exact.
 */
template <typename value_type, unsigned dim>
inline value_type orient_3d (const vector<value_type, dim> & a, const vector<value_type, dim> & b,  const vector<value_type, dim> & c, const vector<value_type, dim> & d)
{
	return (d - a) * ((b - a) ^ (c - a)) ;
}

/**
 * This function predicate the co-planar convex polygon - segment intersection. 
 * We may use http://gpwiki.org/index.php/Polygon_Collision. 
 * Here we check every intersection of edges. 
 */
template <typename value_type, unsigned dim>
	inline bool predicate_polygon_convex_segment_coplanar (const Polygon <value_type, dim> & _pol1, const Segment <value_type, dim> & _seg)
{
	vector<value_type, dim> normal = _pol1.calculate_normal();
	unsigned x, y; 
    select_map_axis(normal, x, y);


	unsigned size1 = _pol1.size();

	Polygon <value_type, dim> _pol1_2d;

	for (unsigned i = 0; i < size1; ++i)
	{
		_pol1_2d.append_vertex (vector<value_type, dim> (_pol1[i][x], _pol1[i][y]) );
	}


	Segment <value_type, dim> _seg_2d; 
	_seg_2d.p0 = vector <value_type, dim> (_seg.p0[x], _seg.p0[y]);
	_seg_2d.p1 = vector <value_type, dim> (_seg.p1[x], _seg.p1[y]);

	std::cout<<_pol1_2d<<std::endl;
	std::cout<<_seg_2d<<std::endl;
	if ((_seg_2d.p0 - _seg_2d.p1).square_module() < epsilon)
	{
		return  predicate_pol_convex_point (_pol1, _seg_2d.p0);
	}
	bool flag1 = predicate_pol_convex_point (_pol1, _seg_2d.p0);
	bool flag2 = predicate_pol_convex_point (_pol1, _seg_2d.p1);
	if (flag1 || flag2) return true; 

	for (unsigned  i = 0, j = size1 - 1; i < size1; j = i++)
	{
		if (predicate_seg_seg_2d(Segment<value_type, dim> (_pol1_2d[i], _pol1_2d[j]), _seg_2d) ) return true; 
	}
	return false; 
	
	//return true; 
}

/**
 * Predicate the interseciton of 2D segments
 */
template<typename value_type, unsigned dim>
	bool predicate_seg_seg_2d (const Segment<value_type, dim> & seg1, const Segment<value_type, dim> & seg2)
{
	value_type d0 = orient_2d(seg1.p0, seg1.p1, seg2.p0);
	value_type d1 = orient_2d(seg1.p0, seg1.p1, seg2.p1);

	value_type d2 = orient_2d(seg2.p0, seg2.p1, seg1.p0);
	value_type d3 = orient_2d(seg2.p0, seg2.p1, seg1.p1);

	if (d0 == 0 && d1 == 0)
	{
		Segment<value_type, dim> s1 = seg1 ;
		Segment<value_type, dim> s2 = seg2 ;
		s1.order(); s2.order ();
		if (s1.p0 < s2.p1 || s2.p0 < s1.p1)  return false; else return false;
	}
	return (d0 * d1 <=0 && d2 * d3 <=0);
}


/**
 * This function predicate the co-planar convex polygon - polygon intersection.
 * We may use http://gpwiki.org/index.php/Polygon_Collision. 
 * Here we check every intersection of edges. 
 */
template <typename value_type, unsigned dim>
inline bool predicate_polygon_polygon_convex_coplanar (const Polygon <value_type, dim> & _pol1, const Polygon <value_type, dim> & _pol2)
{
	
	vector<value_type, dim> normal = _pol1.calculate_normal();
	unsigned x, y; 
    select_map_axis(normal, x, y);


	unsigned size1 = _pol1.size();
	unsigned size2 = _pol2.size(); 

	Polygon <value_type, 2> _pol1_2d, _pol2_2d;

	for (unsigned i = 0; i < size1; ++i)
	{
		_pol1_2d.append_vertex (vector<value_type, 2> (_pol1[i][x], _pol1[i][y]) );
	}

	for (unsigned i = 0; i < size2; ++i)
	{
		_pol2_2d.append_vertex (vector<value_type, 2> (_pol2[i][x], _pol2[i][y]) );
	}

	for (unsigned  i = 0, j = size1 - 1; i < size1; j = i++)
	{
		for (unsigned  m = 0, n = size2 - 1; m < size2; n = m++)
		{
			if (predicate_seg_seg_2d(Segment <value_type, 2> (_pol1_2d[i], _pol1_2d[j]), Segment <value_type, 2> (_pol2_2d[m], _pol2_2d[n])) ) 
			{
				return true; 
			}
		}
	}

	if (is_in_polgyon_2d(_pol1_2d, _pol2_2d[0]) && is_in_polgyon_2d(_pol1_2d, _pol2_2d[1]) && is_in_polgyon_2d(_pol1_2d, _pol2_2d[0]))
	{
		return true; 
	}
	else if (is_in_polgyon_2d(_pol2_2d, _pol1_2d[0]) && is_in_polgyon_2d(_pol2_2d, _pol1_2d[0]) && is_in_polgyon_2d(_pol2_2d, _pol1_2d[0]))
	{
		return true; 
	}
	else 
	{
		for (unsigned i = 0; i < _pol2_2d.size(); ++i)
		{
			if (is_in_polgyon_2d(_pol1_2d, _pol2_2d[i])) 
			{
				return true; 
			}
		}
	}
	return false; 
	
	//return true; 
}
template <typename value_type, unsigned dim>
bool predicate_polygon_polygon_convex (const Polygon <value_type, dim> & _pol1, const Polygon <value_type, dim> & _pol2)
{
	vector<value_type, dim> normal1 = _pol1.calculate_normal();
	vector<value_type, dim> normal2 = _pol2.calculate_normal();

	unsigned size1 = _pol1.size();
	unsigned size2 = _pol2.size(); 
	unsigned u0, u1, u2, u3;
	u0 =  u1 =  u2 = u3 = size1; 

	unsigned coplanar1 = 0;

	unsigned i = 0, j = size1 - 1; 
	for (; i < size1; j = i++)
	{
		value_type d0 = (_pol1[j] - _pol2[0]) * normal2 ; // Predicate
		value_type d1 = (_pol1[i] - _pol2[0]) * normal2 ; // Predicate
		if (std::abs(d1)  < value_type(epsilon)) {coplanar1 ++; }
		if (d0 * d1 <= value_type(epsilon))
		{
			if (d0 >= value_type(epsilon) && d1 < -value_type(epsilon)) { u0 = j ; u1 = i; }
			else if (d0 > value_type(epsilon) && d1 <= -value_type(epsilon)) { u0 = j ; u1 = i; }
			else if (d1 >= value_type(epsilon) && d0 < -value_type(epsilon)){ u3 = i ; u2 = j; }
			else if (d1 > value_type(epsilon) && d0 <= -value_type(epsilon)){ u3 = i ; u2 = j; }
		}
/*
  if (d0 * d1 <= value_type(0))
  {
  if (d0 >= value_type(0) && d1 < value_type(0)) { u0 = j ; u1 = i; }
  else if (d0 > value_type(0) && d1 <= value_type(0)) { u0 = j ; u1 = i; }
  else if (d1 >= value_type(0) && d0 < value_type(0)){ u3 = i ; u2 = j; }
  else if (d1 > value_type(0) && d0 <= value_type(0)){ u3 = i ; u2 = j; }
			
  }
*/
	}
	//for (unsigned i = 0; i < coplanar_id.size(); ++i) std::cout<<coplanar_id[i]<<' ';
	//std::cout<<std::endl;
	if (coplanar1 >= 3) 
	{	
		return (predicate_polygon_polygon_convex_coplanar(_pol1, _pol2));
	}
/*
  else
  if (coplanar == 2)
  {
  return (predicate_polygon_convex_segment_coplanar(_pol2, Segment <value_type, dim> (_pol1[coplanar_id[0]], _pol1[coplanar_id[1]] ) ));
  //throw ("An edge on the face. ");
  }
*/
	if ( u0 == size1 || u2 == size1 ) return false;  
	
	unsigned v0, v1, v2, v3 ;
	v0 = v1 = v2 = v3 = size2; 
	unsigned coplanar2 = 0; 
	for (i = 0, j = size2 - 1; i < size2; j = i++)
	{
		value_type d0 = ( _pol2[j] - _pol1[0] ) * normal1 ; // Predicate
		value_type d1 = ( _pol2[i] - _pol1[0] ) * normal1 ; // Predicate
		if (std::abs(d1)  <value_type(epsilon)) {coplanar2 ++; }
		if (d0 * d1 <= value_type(epsilon))
		{
			if (d0 >= value_type(epsilon) && d1 < -value_type(epsilon)) { v0 = j ; v1 = i; }
			else if (d0 > value_type(epsilon) && d1 <= -value_type(epsilon)) { v0 = j ; v1 = i; }
			else if (d1 >= value_type(epsilon) && d0 < -value_type(epsilon)){ v3 = i ; v2 = j; }
			else if (d1 > value_type(epsilon) && d0 <= -value_type(epsilon)){ v3 = i ; v2 = j; }
		}
/*
  if (d0 * d1 <= value_type(0))
  {
  if (d0 >= value_type(0) && d1 < value_type(0)) { v0 = j ; v1 = i; }
  else if (d0 > value_type(0) && d1 <= value_type(0)) { v0 = j ; v1 = i; }
  else if (d1 >= value_type(0) && d0 < value_type(0)){ v3 = i ; v2 = j; }
  else if (d1 > value_type(0) && d0 <= value_type(0)){ v3 = i ; v2 = j; }
  }
*/
	}
	if (v0 == size2 || v2 == size2 ) return false; 

	//[u0, u1, v0, v1] <=0 Predicate
	value_type d0 = orient_3d(_pol1[u0], _pol1[u1], _pol2[v0], _pol2[v1]);
	//[u2, u3, v3, v2] <=0 Predicate
	value_type d1 = orient_3d(_pol1[u2], _pol1[u3], _pol2[v3], _pol2[v2]);

//	if (coplanar1 !=2 && coplanar2 == 2)
	return (d0 <= 0 && d1 <= 0);
//	else return false; 

}


template <typename value_, unsigned dim>
struct predicate_poly_poly_impl
{
	typedef value_ value_type; 
	typedef Polygon <value_type, dim> polygon_type;
	static bool predicate (const polygon_type & _pol1, const polygon_type & _pol2) 
	{
		return predicate_polygon_polygon_convex(_pol1, _pol2);
	}
	//predicate_polygon_polygon_convex
};

/*
template <>
struct predicate_poly_poly_impl<double>
{

	typedef long double value_type; 
	typedef Polygon <value_type, dim> long_polygon_type;
	typedef Polygon<double> polygon_type;

	static  void convert (const polygon_type & p, long_polygon_type & long_p)
	{
		for (unsigned i = 0; i < p.size(); ++i) 
			long_p.append_vertex(Vector <long double> (p[i][0], p[i][1], p[i][2]));

	}
	static bool predicate (const polygon_type & _pol1, const polygon_type & _pol2) 
	{
		long_polygon_type lp1 , lp2; 
		convert (_pol1, lp1) ;
		convert (_pol2, lp2) ; 
		return predicate_polygon_polygon_convex(lp1, lp2);
	}
	//predicate_polygon_polygon_convex
};
*/
/**
 * This function handle the strictly convex polygon - polygon. 
 - The general method is described in http://home.arcor.de/philippe.guigue/Sample_codes/poly_poly_intersection.ps.gz
 */
template <typename value_type, unsigned dim>
inline bool predicate_polygon_polygon (const Polygon <value_type, dim> & _pol1, const Polygon <value_type, dim> & _pol2)
{
	return predicate_poly_poly_impl<value_type, dim>::predicate(_pol1, _pol2) ;
}



//////////////////////////////////////////////////////////////////////////////////
// HERE ARE THE PREDICATORS                                  /////////////////////
// Acutally, they are not truly predicates.                  /////////////////////
//////////////////////////////////////////////////////////////////////////////////


template <typename value_type, unsigned dim>
	bool predicate_segment_box (const Segment <value_type, dim> & _segment, const Box <value_type, dim> & box)
{
	// Check if one end is in the box.
	if (box.isInside (_segment.p0) || box.isInside(_segment.p1)) return true; 
	
	
	value_type fAWdU[3], fADdU[3], fAWxDdU[3], fRhs;
	vector<value_type, dim> kSDir = value_type(0.5) * (_segment.p1 - _segment.p0);
	vector<value_type, dim> kSCen = _segment.p0 + kSDir;

	vector<value_type, dim> kDiff = kSCen - box.center;

	fAWdU[0] = std::abs(kSDir[0]);
	fADdU[0] = std::abs(kDiff[0]);
	fRhs = box.extent[0] + fAWdU[0];
	if (fADdU[0] > fRhs)
		return false;

	fAWdU[1] = std::abs(kSDir[1]);
	fADdU[1] = std::abs(kDiff[1]);
	fRhs = box.extent[1] + fAWdU[1];
	if (fADdU[1] > fRhs)
		return false;

	fAWdU[2] = std::abs(kSDir[2]);
	fADdU[2] = std::abs(kDiff[2]);
	fRhs = box.extent[2] + fAWdU[2];
	if (fADdU[2] > fRhs)
		return false;

	vector<value_type, dim> kWxD = kSDir ^ kDiff;

	fAWxDdU[0] = std::abs(kWxD[0]);
	fRhs = box.extent[1] * fAWdU[2] + box.extent[2] * fAWdU[1];
	if (fAWxDdU[0] > fRhs)
		return false;

	fAWxDdU[1] = std::abs(kWxD[1]);
	fRhs = box.extent[0] * fAWdU[2] + box.extent[2] * fAWdU[0];
	if (fAWxDdU[1] > fRhs)
		return false;

	fAWxDdU[2] = std::abs(kWxD[2]);
	fRhs = box.extent[0] * fAWdU[1] + box.extent[1] * fAWdU[0];
	if (fAWxDdU[2] > fRhs)
		return false;

    return true;

}

/**
 * This segment must be oriented. The polygon is also oriented. 
 * p0 is above the polygon. p1 is below the polygon. 
 * here we don't consider th coplanar cases. 
 */
template <typename value_type>
	bool predicate_convex_polygon_segment (const Polygon <value_type, 3> & _pol, const Segment <value_type, 3> & _seg)
{

	if ( orient_3d (_pol[0], _pol[1], _pol[2], _seg.p0) * orient_3d (_pol[0], _pol[1], _pol[2], _seg.p1) > 0 ) return false; 


//	predicate_polygon_convex_segment_coplanar(_pol, _seg) ;
	unsigned size = _pol.size() ;
	for (unsigned i = 0; i < size; ++i)
	{
		if (orient_3d (_pol[i], _pol[(i+1)%size], _seg.p0, _seg.p1) > 0) return false  ; 
	}
	return true; 
}


template <typename value_type, unsigned dim>
	bool predicate_polygon_segment (const Polygon <value_type, dim> & _pol, const Segment <value_type, dim> & _segment)
{
	Segment <value_type, dim> seg (_segment.p1, _segment.p0); 
	return predicate_convex_polygon_segment(_pol, _segment) || predicate_convex_polygon_segment(_pol, seg);
}




/**
 * Predicate the intersection between Polygon and a Box. 
 - Check if any vertex of polygon is inside the box. 
 * @link http://jgt.akpeters.com/papers/AkenineMoller01/tribox.html Triangle overlap with box @endlink
 */
#define FINDMINMAX(x0,x1,x2,min,max)			\
	min = max = x0;								\
	if(x1<min) min=x1;							\
	if(x1>max) max=x1;							\
	if(x2<min) min=x2;							\
	if(x2>max) max=x2;

template <typename value_type, unsigned dim>
int planeBoxOverlap(const vector<value_type, dim> &  normal, const value_type & d, const vector<value_type, dim> & maxbox)
{
	vector<value_type, dim> vmin ,vmax ;
	for(int q=0; q <3 ; ++q)
	{
		if(normal[q]>0.0f)
		{
			vmin[q]=-maxbox[q];
			vmax[q]=maxbox[q];
		}
		else
		{
			vmin[q]=maxbox[q];
			vmax[q]=-maxbox[q];
		}
	}
//	if(normal * vmin + d > 0.0f) return 0;
//	if(normal * vmax + d >=0.0f) return 1;

	value_type test1 = normal * vmin + d;
	value_type test2 = normal * vmax + d;
	if(test1 > epsilon) return 0;
	if(std::abs(test2) <= epsilon || test2 >=value_type(0.0) ) return 1;
	
  
	return 0;
}


// ======================== X-tests ========================
#define AXISTEST_X01(a, b, fa, fb)						\
	p0 = a*v0[1] - b*v0[2];								\
	p2 = a*v2[1] - b*v2[2];								\
	if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;}	\
	rad = fa * boxhalfsize[1] + fb * boxhalfsize[2];	\
	if(min>rad + epsilon || max<-rad - epsilon) return 0;

#define AXISTEST_X2(a, b, fa, fb)						\
	p0 = a*v0[1] - b*v0[2];								\
	p1 = a*v1[1] - b*v1[2];								\
	if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;}	\
	rad = fa * boxhalfsize[1] + fb * boxhalfsize[2];	\
	if(min>rad + epsilon || max<-rad - epsilon) return 0;

// ======================== Y-tests ========================
#define AXISTEST_Y02(a, b, fa, fb)						\
	p0 = -a*v0[0] + b*v0[2];							\
	p2 = -a*v2[0] + b*v2[2];							\
	if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;}	\
	rad = fa * boxhalfsize[0] + fb * boxhalfsize[2];	\
	if(min>rad + epsilon || max<-rad - epsilon) return 0;

#define AXISTEST_Y1(a, b, fa, fb)						\
	p0 = -a*v0[0] + b*v0[2];							\
	p1 = -a*v1[0] + b*v1[2];							\
	if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;}	\
	rad = fa * boxhalfsize[0] + fb * boxhalfsize[2];	\
	if(min>rad + epsilon || max<-rad - epsilon) return 0;

// ======================== Z-tests ========================

#define AXISTEST_Z12(a, b, fa, fb)						\
	p1 = a*v1[0] - b*v1[1];								\
	p2 = a*v2[0] - b*v2[1];								\
	if(p2<p1) {min=p2; max=p1;} else {min=p1; max=p2;}	\
	rad = fa * boxhalfsize[0] + fb * boxhalfsize[1];	\
	if(min>rad + epsilon|| max<-rad - epsilon) return 0;

#define AXISTEST_Z0(a, b, fa, fb)						\
	p0 = a*v0[0] - b*v0[1];								\
	p1 = a*v1[0] - b*v1[1];								\
	if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;}	\
	rad = fa * boxhalfsize[0] + fb * boxhalfsize[1];	\
	if(min>rad + epsilon|| max<-rad - epsilon) return 0;

/*
// ======================== X-tests ========================
#define AXISTEST_X01(a, b, fa, fb)						\
	p0 = a*v0[1] - b*v0[2];								\
	p2 = a*v2[1] - b*v2[2];								\
	if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;}	\
	rad = fa * boxhalfsize[1] + fb * boxhalfsize[2];	\
	if(min>rad  || max<-rad ) return 0;

#define AXISTEST_X2(a, b, fa, fb)						\
	p0 = a*v0[1] - b*v0[2];								\
	p1 = a*v1[1] - b*v1[2];								\
	if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;}	\
	rad = fa * boxhalfsize[1] + fb * boxhalfsize[2];	\
	if(min>rad  || max<-rad ) return 0;

// ======================== Y-tests ========================
#define AXISTEST_Y02(a, b, fa, fb)						\
	p0 = -a*v0[0] + b*v0[2];							\
	p2 = -a*v2[0] + b*v2[2];							\
	if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;}	\
	rad = fa * boxhalfsize[0] + fb * boxhalfsize[2];	\
	if(min>rad || max<-rad ) return 0;

#define AXISTEST_Y1(a, b, fa, fb)						\
	p0 = -a*v0[0] + b*v0[2];							\
	p1 = -a*v1[0] + b*v1[2];							\
	if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;}	\
	rad = fa * boxhalfsize[0] + fb * boxhalfsize[2];	\
	if(min>rad || max<-rad ) return 0;

// ======================== Z-tests ========================

#define AXISTEST_Z12(a, b, fa, fb)						\
	p1 = a*v1[0] - b*v1[1];								\
	p2 = a*v2[0] - b*v2[1];								\
	if(p2<p1) {min=p2; max=p1;} else {min=p1; max=p2;}	\
	rad = fa * boxhalfsize[0] + fb * boxhalfsize[1];	\
	if(min>rad || max<-rad ) return 0;

#define AXISTEST_Z0(a, b, fa, fb)						\
	p0 = a*v0[0] - b*v0[1];								\
	p1 = a*v1[0] - b*v1[1];								\
	if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;}	\
	rad = fa * boxhalfsize[0] + fb * boxhalfsize[1];	\
	if(min>rad || max<-rad ) return 0;
*/

template <typename value_type, unsigned dim>
bool triBoxOverlap(const vector<value_type, dim> & boxcenter, const vector<value_type, dim> & boxhalfsize,const Polygon <value_type, dim> & triverts)
{

	/*    use separating axis theorem to test overlap between triangle and box */
	/*    need to test for overlap in these directions: */
	/*    1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */
	/*       we do not even need to test these) */
	/*    2) normal of the triangle */
	/*    3) crossproduct(edge from tri, {x,y,z}-directin) */
	/*       this gives 3x3=9 more tests */
	vector<value_type, dim> v0, v1, v2 ;
	vector<value_type, dim> axis;
	value_type min,max,d,p0,p1,p2,rad,fex,fey,fez;  
	vector<value_type, dim> normal, e0, e1, e2;

	/* This is the fastest branch on Sun */
	/* move everything so that the boxcenter is in (0,0,0) */
	v0 = triverts[0] - boxcenter; 
	v1 = triverts[1] - boxcenter; 
	v2 = triverts[2] - boxcenter; 

	/* compute triangle edges */
	e0 = v1 - v0;
	e1 = v2 - v1;
	e2 = v0 - v2;


	/* Bullet 3:  */
	/*  test the 9 tests first (this was faster) */
	fex = fabs(e0[0]);
	fey = fabs(e0[1]);
	fez = fabs(e0[2]);
	AXISTEST_X01(e0[2], e0[1], fez, fey);
	AXISTEST_Y02(e0[2], e0[0], fez, fex);
	AXISTEST_Z12(e0[1], e0[0], fey, fex);

	fex = fabs(e1[0]);
	fey = fabs(e1[1]);
	fez = fabs(e1[2]);
	AXISTEST_X01(e1[2], e1[1], fez, fey);
	AXISTEST_Y02(e1[2], e1[0], fez, fex);
	AXISTEST_Z0 (e1[1], e1[0], fey, fex);

	fex = fabs(e2[0]);
	fey = fabs(e2[1]);
	fez = fabs(e2[2]);
	AXISTEST_X2 (e2[2], e2[1], fez, fey);
	AXISTEST_Y1 (e2[2], e2[0], fez, fex);
	AXISTEST_Z12(e2[1], e2[0], fey, fex);

	/* Bullet 1: */
	/*  first test overlap in the {x,y,z}-directions */
	/*  find min, max of the triangle each direction, and test for overlap in */
	/*  that direction -- this is equivalent to testing a minimal AABB around */
	/*  the triangle against the AABB */

	/* test in X-direction */
	FINDMINMAX(v0[0],v1[0],v2[0],min,max);
	if(min>boxhalfsize[0] + epsilon || max<-boxhalfsize[0] - epsilon) return false;

	/* test in Y-direction */
	FINDMINMAX(v0[1],v1[1],v2[1],min,max);
	if(min>boxhalfsize[1] + epsilon|| max<-boxhalfsize[1] - epsilon) return false;

	/* test in Z-direction */
	FINDMINMAX(v0[2],v1[2],v2[2],min,max);
	if(min>boxhalfsize[2] + epsilon || max<-boxhalfsize[2] - epsilon) return false;

	/* Bullet 2: */
	/*  test if the box intersects the plane of the triangle */
	/*  compute plane equation of triangle: normal*x+d=0 */
	normal = e0 ^ e1;
	d = normal * v0 ;
	if(!planeBoxOverlap(normal,d,boxhalfsize)) return false;

	return true;   /* box and triangle overlaps */
}

template <typename value_type, unsigned dim>
	bool predicate_polygon_box (const Polygon <value_type, dim> & _pol, const Box <value_type, dim> & _box)
{
	// Check if any vertex of polygon is inside the box. 
	// The idea is we split the convex polygon to be triangle and do the test.
	// If the polygon is not convex. WE will do triangulation ? Who knows. 
	if (_pol.size() !=3 ) throw ("Polygon box implementation is pending. ");
	return  triBoxOverlap(_box.center, _box.extent, _pol) ;
}



/**
 * This function now handle triangle intersects triangle.
 */
template <typename value_type, unsigned dim>
bool predicate_tri_tri (const Polygon <value_type, dim> & _pol1, const Polygon <value_type, dim> & _pol2)
{
	/*
	double p1[3], q1[3], r1[3];
	double p2[3], q2[3], r2[3];

	p1[0] = _pol1[0][0]; p1[1] = _pol1[0][1]; p1[2] = _pol1[0][2];
	q1[0] = _pol1[1][0]; q1[1] = _pol1[1][1]; q1[2] = _pol1[1][2];
	r1[0] = _pol1[2][0]; r1[1] = _pol1[2][1]; r1[2] = _pol1[2][2];
	
	p2[0] = _pol2[0][0]; p2[1] = _pol2[0][1]; p2[2] = _pol2[0][2];
	q2[0] = _pol2[1][0]; q2[1] = _pol2[1][1]; q2[2] = _pol2[1][2];
	r2[0] = _pol2[2][0]; r2[1] = _pol2[2][1]; r2[2] = _pol2[2][2];
	
	return tri_tri_overlap_test_3d(p1, q1, r1, p2, q2, r2);
	*/
	return tri_tri_overlap_test_3d(_pol1[0], _pol1[1], _pol1[2], _pol2[0], _pol2[1], _pol2[2]);

}

/**
 * Decide along which axis to do 2D map. 
 * @return 0-x, 1-y, 2-z; 
 */
template <typename value_type, unsigned dim>
unsigned select_map_axis(const vector<value_type, dim> & normal,  unsigned & x, unsigned & y) 
{
	value_type ax, ay, az ;
	unsigned coord ; // coord to be ignored. 0-x, 1-y, 2-z
		
	ax = std::abs(normal[0]) ;
	ay = std::abs(normal[1]) ;
	az = std::abs(normal[2]) ;
		
	coord = 2; 
	if (ax > ay )
	{
		if (ax > az) coord = 0;
	}
	else
	{
		if (ay > az) coord = 1 ;
	}

	x = (coord + 1) % 3; 
	y = (coord + 2) % 3;

	return coord; 
}

/**
 * Map 3D points to 2D points. 
 * @pre {select_map_axis}
 */
template <typename value_type, unsigned dim>
void map_3D_point_to_2D_point (unsigned x, unsigned y, const vector<value_type, dim> & pt3d, vector<value_type, dim> & pt2d)
{
}



/**
 * Map 2D points to 3D points. 
 * @pre {select_map_axis}
 */
template <typename value_type, unsigned dim>
	void map_2D_point_to_3D_point (unsigned x, unsigned y, const Plane <value_type, dim> & plane, const vector<value_type, dim> & pt2d, vector<value_type, dim> & pt3d)
{
	value_type a, b, c, d; 
	plane.get_parameter (a,b,c,d); 
	if (x == 0 && y == 1)
	{
		pt3d = vector<value_type, dim> (pt2d[0], pt2d[1], ( -d - a * pt2d[0] - b * pt2d[1] ) / c);
	} else
	if (x == 2 && y == 0)
	{
		pt3d = vector<value_type, dim> (pt2d[1], ( -d - a * pt2d[1] - c * pt2d[0] ) / b, pt2d[0]);
	} else
	if (x == 1 && y == 2)
	{
		pt3d = vector<value_type, dim> ( ( -d - b * pt2d[0] - c * pt2d[1] ) / a, pt2d[0], pt2d[1]);
	}
}

/**
 * predicate the ray and the triangle.
 * http://jgt.akpeters.com/papers/Chirkov05/C2005.zip
 * @pre The direction of the ray must be normalized. 
 */
#define EPSILON 1e-14 

#define CROSS(dest,v1,v2) \
          dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
          dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
          dest[2]=v1[0]*v2[1]-v1[1]*v2[0];

#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])

#define SUB(dest,v1,v2)\
          dest[0]=v1[0]-v2[0]; \
          dest[1]=v1[1]-v2[1]; \
          dest[2]=v1[2]-v2[2]; 
template <typename value_type, unsigned dim>
bool _intersect_ray_tri(value_type orig[3], value_type dir[3],
					  value_type vert0[3], value_type vert1[3], value_type vert2[3],
					  value_type *t, value_type *u, value_type *v)
{

        double edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
        double det,inv_det;

        /* find vectors for two edges sharing rt->v0 */
        SUB(edge1, vert1, vert0);
		SUB(edge2, vert2, vert0);

        /* begin calculating determinant - also used to calculate U parameter */
        CROSS(pvec, dir, edge2);

        /* if determinant is near zero, ray lies in plane of triangle */
        det = DOT(edge1, pvec);

        /* calculate distance from rt->v0 to ray origin */
        SUB(tvec, orig, vert0);
        inv_det = 1.0 / det;

        if (det > epsilon)
        {
                /* calculate U parameter and test bounds */
                *u = DOT(tvec, pvec);
                if (*u < 0.0 || *u > det)
                        return 0;

                /* prepare to test V parameter */
                CROSS(qvec, tvec, edge1);

                /* calculate V parameter and test bounds */
                *v = DOT(dir, qvec);
                if (*v < 0.0 || *u + *v > det)
                        return 0;

        }
        else if(det < - epsilon)
        {
                /* calculate U parameter and test bounds */
                *u = DOT(tvec, pvec);
                if (*u > 0.0 || *u < det)
                        return 0;

                /* prepare to test V parameter */
                CROSS(qvec, tvec, edge1);

                /* calculate V parameter and test bounds */
                *v = DOT(dir, qvec) ;
                if (*v > 0.0 || *u + *v < det)
                        return 0;
        }
        else 
		{
			//throw ("ray is paralle to the plane of the triangle");
			return 0;  /* ray is parallell to the plane of the triangle */
		}

        /* calculate t, ray intersects triangle */
        *t = DOT(edge2, qvec) * inv_det;
        //if(*t<0.0 || *t>1.0)      return 0; // BUG. FIXED. 
		if (*t < - epsilon ) return 0; 
/*
		std::cout<<"u "<<*u<<" v "<<*v<< " t "<<*t<<std::endl;
		*u = std::abs((*u) * det);
		*v = std::abs((*v) * det);
		*/
        return 1;

}

/**
 * predicate the ray and the triangle 
 */
template <typename value_type, unsigned dim>
	bool predicate_ray_tri (const Ray<value_type, dim> & _ray, const Polygon <value_type, dim> & _pol)
{
	
	if (_pol.size() != 3) throw ("Ray - triangle perdicate is not called with a triangle. ");
	value_type orig[3], dir[3];
	value_type vert0[3], vert1[3],  vert2[3]; 
	value_type t, u, v ;
	vector<value_type, dim> p, d ;
	p = _ray.get_point();
	d = _ray.get_direction ();

	for (int i = 0; i < 3; ++i)
	{
		orig[i] = p[i];
		dir[i]  = d[i];
		vert0[i] = _pol[0][i];
		vert1[i] = _pol[1][i];
		vert2[i] = _pol[2][i];
	}
	return _intersect_ray_tri(orig, dir, vert0, vert1, vert2, & t, & u, &v);
	
}

/**
 * predicate the ray and the triangle 
 */
template <typename value_type, unsigned dim>
	bool predicate_polygon_ray (const Polygon <value_type, dim> & _pol, const Ray<value_type, dim> & _ray)
{
	if (_pol.size() > 3) throw ("Ray- polygon perdicate is not implemented. ");
	return predicate_ray_tri(_ray, _pol);
}

/**
 * Ray-box intersection using IEEE numerical properties to ensure that the
 * test is both robust and efficient, as described in:
 *
 -     Amy Williams, Steve Barrus, R. Keith Morley, and Peter Shirley
 *      "An Efficient and Robust Ray-Box Intersection Algorithm"
 *      Journal of graphics tools, 10(1):49-54, 2005
 *
 *
 - Note. If the origin of the ray falls on the box face. We also think they do not intersect. 
 - The value_type type must support 1.0 / 0.0. Core library doesn't support this. 
 */
template <typename value_type, unsigned dim>
	bool predicate_box_ray (const Box<value_type, dim> & _box, const Ray<value_type, dim> & _ray)
{
	/*
	const value_type t = value_type(1e10);
	vector<value_type, dim> origin = _ray.get_point();
	vector<value_type, dim> dir = _ray.get_direction();

	Segment <value_type, dim> seg (origin, origin + t * dir);
	return predicate_segment_box(seg, _box);
	*/
	
	const value_type t0 = value_type(0);     // This value can be negative. 
	const value_type t1 = value_type(1e10);  // Change this value to extend the lenght of ray.
	value_type tmin, tmax, tymin, tymax, tzmin, tzmax;
	int sign[3];
	vector<value_type, dim> inv_dir (3);
	vector<value_type, dim> origin = _ray.get_point();
	vector<value_type, dim> dir = _ray.get_direction();

	vector<value_type, dim> parameters[2];
	parameters[0] = _box.getMin();
	parameters[1] = _box.getMax();


	inv_dir[0] = value_type(1.0)/ dir[0];
    inv_dir[1] = value_type(1.0)/ dir[1];
	inv_dir[2] = value_type(1.0)/ dir[2];


	sign[0] = (inv_dir[0] < 0 );
	sign[1] = (inv_dir[1] < 0 );
	sign[2] = (inv_dir[2] < 0 );


	tmin = (parameters[sign[0]][0] -  origin[0]) * inv_dir[0];
	tmax = (parameters[1- sign[0]][0] - origin[0]) * inv_dir[0];
	tymin = (parameters[sign[1]][1] - origin[1]) * inv_dir[1];
	tymax = (parameters[1-sign[1]][1] - origin[1]) * inv_dir[1];
	if ( (tmin > tymax) || (tymin > tmax) ) 
		return false;
	if (tymin > tmin)
		tmin = tymin;
	if (tymax < tmax)
		tmax = tymax;
	tzmin = (parameters[sign[2]][2] - origin[2]) * inv_dir[2];
	tzmax = (parameters[1-sign[2]][2] - origin[2]) * inv_dir[2];
	if ( (tmin > tzmax) || (tzmin > tmax) ) 
		return false;
	if (tzmin > tmin)
		tmin = tzmin;
	if (tzmax < tmax)
		tmax = tzmax;
	//return ( (tmin < t1) && (tmax > t0) ); // The original cannot falls on the box. 
	//return ( (tmin < t1) && (tmax >= t0) ); // The original can falls on the box. 
	return ( (tmin < t1) && (tmax >= t0 || abs(tmax - t0) <= epsilon) ); // The original can falls on the box. 
}

template<typename value_type>
value_type distance2(const vector<value_type, 3> & _pt, const Box<value_type, 3> &_box, vector<value_type, 3> & _nearestPoint)
{
        value_type x0 = _box.center.x - _box.extent.x;
        value_type x1 = _box.center.x + _box.extent.x;
        value_type y0 = _box.center.y - _box.extent.y;
        value_type y1 = _box.center.y + _box.extent.y;
        value_type z0 = _box.center.z - _box.extent.z;
        value_type z1 = _box.center.z + _box.extent.z;

        vector<value_type, 3> closest = _box.center;
        if (_pt.x < x0)
                closest.x = x0;
        else if (_pt.x > x1)
                closest.x = x1;
        else
                closest.x = _pt.x;

        if (_pt.y < y0)
                closest.y = y0;
        else if (_pt.y > y1)
                closest.y = y1;
        else
                closest.y = _pt.y;

        if (_pt.z < z0)
                closest.z = z0;
        else if (_pt.z > z1)
                closest.z = z1;
        else
                closest.z = _pt.z;
       _nearestPoint = closest;

        vector<value_type, 3> ray = _pt - closest;
        value_type sqrDist = ray * ray;
        return sqrDist;
}

template<typename value_type>
value_type distance2(const vector<value_type, 3> & _pt, const Box<value_type, 3> &_box)
{
	vector<value_type, 3> _nearestpoint; 
	return distance2(_pt, _box, _nearestpoint);
}


template<typename value_type>
value_type distance(const vector<value_type, 3> & _pt, const Box<value_type, 3> &_box)
{
	return sqrt (distance2(_pt, _box));
}


template<typename value_type>
value_type distance(const vector<value_type, 3> & _pt, const Polygon<value_type, 3> &_pol)
{
	Plane <value_type, 3> plane(_pol);
	return plane.dist(_pt);
}




template <typename value_type>
bool predicate_sphere_box (const Sphere<value_type, 3> & sphere,  const Box<value_type, 3> & box)
{
	return distance2 (sphere.get_center(), box) < sphere.get_radius() * sphere.get_radius(); 
}

template <typename value_type>
bool predicate_sphere_triangle(const vector<value_type, 3> &center, value_type radius, const vector<value_type, 3> &pt0, const vector<value_type, 3> &pt1, const vector<value_type, 3> &pt2, value_type *dist)
{
	vector<value_type, 3> triOrg = pt0;
	vector<value_type, 3> triEdge0 = pt1 - pt0;
	vector<value_type, 3> triEdge1 = pt2 - pt0;

	vector<value_type, 3> kDiff = triOrg - center;
	value_type fA00 = triEdge0.module2();  // 
	value_type fA01 = triEdge0* triEdge1;
	value_type fA11 = triEdge1.module2();
	value_type fB0 = kDiff * triEdge0;
	value_type fB1 = kDiff * triEdge1;
	value_type fC = kDiff.module2();
	value_type fDet = (value_type) fabs(fA00 * fA11 - fA01 * fA01);
	value_type fS = fA01 * fB1 - fA11 * fB0;
	value_type fT = fA01 * fB0 - fA00 * fB1;
	value_type sqrDist;

	if (fS + fT <= fDet) {
		if (fS < 0.0f) {
			if (fT < 0.0f) {
				// region 4
				if (fB0 < 0.0f) {
					fT = 0.0f;
					if (-fB0 >= fA00) {
						fS = 1.0f;
						sqrDist = fA00 + 2.0f * fB0 + fC;
					}
					else {
						fS = -fB0 / fA00;
						sqrDist = fB0 * fS + fC;
					}
				}
				else {
					fS = 0.0f;
					if (fB1 >= 0.0f) {
						fT = 0.0f;
						sqrDist = fC;
					}
					else if (-fB1 >= fA11) {
						fT = 1.0f;
						sqrDist = fA11 + 2.0f * fB1 + fC;
					}
					else {
						fT = -fB1 / fA11;
						sqrDist = fB1 * fT + fC;
					}
				}
			}
			else {
				// region 3
				fS = 0.0f;
				if (fB1 >= 0.0f) {
					fT = 0.0f;
					sqrDist = fC;
				}
				else if (-fB1 >= fA11) {
					fT = 1.0f;
					sqrDist = fA11 + 2.0f * fB1 + fC;
				}
				else {
					fT = -fB1 / fA11;
					sqrDist = fB1 * fT + fC;
				}
			}
		}
		else if (fT < 0.0f) {
			// region 5
			fT = 0.0f;
			if (fB0 >= 0.0f) {
				fS = 0.0f;
				sqrDist = fC;
			}
			else if (-fB0 >= fA00) {
				fS = 1.0f;
				sqrDist = fA00 + 2.0f * fB0 + fC;
			}
			else {
				fS = -fB0 / fA00;
				sqrDist = fB0 * fS + fC;
			}
		}
		else {
			// region 0
            // minimum at interior point
			value_type fInvDet = 1.0f / fDet;
			fS *= fInvDet;
			fT *= fInvDet;
			sqrDist = fS * (fA00 * fS + fA01 * fT + 2.0f * fB0) +
					  fT * (fA01 * fS + fA11 * fT + 2.0f * fB1) + fC;
		}
	}
	else {
		value_type fTmp0, fTmp1, fNumer, fDenom;
		
		if (fS < 0.0f)  {
			// region 2
			fTmp0 = fA01 + fB0;
			fTmp1 = fA11 + fB1;
			if (fTmp1 > fTmp0) {
				fNumer = fTmp1 - fTmp0;
				fDenom = fA00 - 2.0f * fA01 + fA11;
				if (fNumer >= fDenom) {
					fS = 1.0f;
					fT = 0.0f;
					sqrDist = fA00 + 2.0f * fB0 + fC;
				}
				else {
					fS = fNumer / fDenom;
					fT = 1.0f - fS;
					sqrDist = fS * (fA00 * fS + fA01 * fT + 2.0f * fB0) +
							  fT * (fA01 * fS + fA11 * fT + 2.0f * fB1) + fC;
				}
			}
			else {
				fS = 0.0f;
				if (fTmp1 <= 0.0f) {
					fT = 1.0f;
					sqrDist = fA11 + 2.0f * fB1 + fC;
				}
				else if (fB1 >= 0.0f) {
					fT = 0.0f;
					sqrDist = fC;
				}
				else {
					fT = -fB1 / fA11;
					sqrDist = fB1 * fT + fC;
				}
			}
		}
		else if (fT < 0.0f) {
			// region 6
			fTmp0 = fA01 + fB1;
			fTmp1 = fA00 + fB0;
			if (fTmp1 > fTmp0) {
				fNumer = fTmp1 - fTmp0;
				fDenom = fA00 - 2.0f * fA01 + fA11;
				if (fNumer >= fDenom) {
					fT = 1.0f;
					fS = 0.0f;
					sqrDist = fA11 + 2.0f * fB1 + fC;
				}
				else {
					fT = fNumer / fDenom;
					fS = 1.0f - fT;
					sqrDist = fS * (fA00 * fS + fA01 * fT + 2.0f * fB0) +
							  fT * (fA01 * fS + fA11 * fT + 2.0f * fB1) + fC;
				}
			}
			else {
				fT = 0.0f;
				if (fTmp1 <= 0.0f) {
					fS = 1.0f;
					sqrDist = fA00 + 2.0f * fB0 + fC;
				}
				else if (fB0 >= 0.0f) {
					fS = 0.0f;
					sqrDist = fC;
				}
				else {
					fS = -fB0 / fA00;
					sqrDist = fB0 * fS + fC;
				}
			}
		}
		else {
			// region 1
			fNumer = fA11 + fB1 - fA01 - fB0;
			if (fNumer <= 0.0f) {
				fS = 0.0f;
				fT = 1.0f;
				sqrDist = fA11 + 2.0f * fB1 + fC;
			}
			else {
				fDenom = fA00 - 2.0f * fA01 + fA11;
				if (fNumer >= fDenom) {
					fS = 1.0f;
					fT = 0.0f;
					sqrDist = fA00 + 2.0f * fB0 + fC;
				}
				else {
					fS = fNumer / fDenom;
					fT = 1.0f - fS;
					sqrDist = fS * (fA00 * fS + fA01 * fT + 2.0f * fB0) +
							  fT * (fA01 * fS + fA11 * fT + 2.0f * fB1) + fC;
				}
			}
		}
	}

	sqrDist = (value_type) fabs(sqrDist);

	if (dist)
		*dist = (value_type) sqrt(sqrDist);

	value_type radius2 = radius * radius;
	return sqrDist < radius2 ? true : false;
}

template <typename value_type>
	bool predicate_polygon_sphere(const Polygon<value_type, 3> & polygon, const Sphere <value_type, 3> & sphere)
{
	ASSERT (polygon.size() == 3);
	value_type dist; 
	return predicate_sphere_triangle(sphere.get_center(), sphere.get_radius(), polygon[0], polygon[1], polygon[2], & dist);	
}

/*
template <typename value_type, unsigned dim>
	struct predicate_sphere_box : public binary_function<Sphere<value_type, dim >,  Box<value_type, dim>, bool > 
{
	bool operator () (Sphere<value_type, dim >,  Box<value_type, dim >){}
};
*/
	 }
#endif
